home *** CD-ROM | disk | FTP | other *** search
/ BBS in a Box 3 / BBS in a box - Trilogy III.iso / Files / Prog / D-G / GemsI / Original / MatrixInversion.c < prev    next >
Encoding:
C/C++ Source or Header  |  1992-06-16  |  4.8 KB  |  182 lines  |  [TEXT/MPS ]
  1. /*
  2. Matrix Inversion
  3. by Richard Carling
  4. from "Graphics Gems", Academic Press, 1990
  5. */
  6.     
  7. /* 
  8.  *   inverse( original_matrix, inverse_matrix )
  9.  * 
  10.  *    calculate the inverse of a 4x4 matrix
  11.  *
  12.  *     -1     
  13.  *     A  = ___1__ adjoint A
  14.  *         det A
  15.  */
  16.  
  17. #include "GraphicsGems.h"
  18. inverse( in, out ) matrix4 *in, *out;
  19. {
  20.     int i, j;
  21.     double det, det4x4();
  22.  
  23.     /* calculate the adjoint matrix */
  24.  
  25.     adjoint( in, out );
  26.  
  27.     /*  calculate the 4x4 determinent
  28.      *  if the determinent is zero, 
  29.      *  then the inverse matrix is not unique.
  30.      */
  31.  
  32.     det = det4x4( out );
  33.  
  34.     if ( fabs( det ) < SMALL_NUMBER) {
  35.         printf("Non-singular matrix, no inverse!\n");
  36.         exit();
  37.     }
  38.  
  39.     /* scale the adjoint matrix to get the inverse */
  40.  
  41.     for (i=0; i<4; i++)
  42.         for(j=0; j<4; j++)
  43.         out->element[i][j] = out->element[i][j] / det;
  44. }
  45.  
  46.  
  47. /* 
  48.  *   adjoint( original_matrix, inverse_matrix )
  49.  * 
  50.  *     calculate the adjoint of a 4x4 matrix
  51.  *
  52.  *      Let  a   denote the minor determinant of matrix A obtained by
  53.  *           ij
  54.  *
  55.  *      deleting the ith row and jth column from A.
  56.  *
  57.  *                    i+j
  58.  *     Let  b   = (-1)    a
  59.  *          ij            ji
  60.  *
  61.  *    The matrix B = (b  ) is the adjoint of A
  62.  *                     ij
  63.  */
  64.  
  65. adjoint( in, out ) matrix4 *in; matrix4 *out;
  66. {
  67.     double a1, a2, a3, a4, b1, b2, b3, b4;
  68.     double c1, c2, c3, c4, d1, d2, d3, d4;
  69.     double det3x3();
  70.  
  71.     /* assign to individual variable names to aid  */
  72.     /* selecting correct values  */
  73.  
  74.     a1 = in->element[0][0]; b1 = in->element[0][1]; 
  75.     c1 = in->element[0][2]; d1 = in->element[0][3];
  76.  
  77.     a2 = in->element[1][0]; b2 = in->element[1][1]; 
  78.     c2 = in->element[1][2]; d2 = in->element[1][3];
  79.  
  80.     a3 = in->element[2][0]; b3 = in->element[2][1];
  81.     c3 = in->element[2][2]; d3 = in->element[2][3];
  82.  
  83.     a4 = in->element[3][0]; b4 = in->element[3][1]; 
  84.     c4 = in->element[3][2]; d4 = in->element[3][3];
  85.  
  86.  
  87.     /* row column labeling reversed since we transpose rows & columns */
  88.  
  89.     out->element[0][0]  =   det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
  90.     out->element[1][0]  = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
  91.     out->element[2][0]  =   det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
  92.     out->element[3][0]  = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
  93.         
  94.     out->element[0][1]  = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
  95.     out->element[1][1]  =   det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
  96.     out->element[2][1]  = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
  97.     out->element[3][1]  =   det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
  98.         
  99.     out->element[0][2]  =   det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
  100.     out->element[1][2]  = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
  101.     out->element[2][2]  =   det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
  102.     out->element[3][2]  = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
  103.         
  104.     out->element[0][3]  = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
  105.     out->element[1][3]  =   det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
  106.     out->element[2][3]  = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
  107.     out->element[3][3]  =   det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
  108. }
  109. /*
  110.  * double = det4x4( matrix )
  111.  * 
  112.  * calculate the determinent of a 4x4 matrix.
  113.  */
  114. double det4x4( m ) matrix4 *m;
  115. {
  116.     double ans;
  117.     double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3,             d4;
  118.     double det3x3();
  119.  
  120.     /* assign to individual variable names to aid selecting */
  121.     /*  correct elements */
  122.  
  123.     a1 = m->element[0][0]; b1 = m->element[0][1]; 
  124.     c1 = m->element[0][2]; d1 = m->element[0][3];
  125.  
  126.     a2 = m->element[1][0]; b2 = m->element[1][1]; 
  127.     c2 = m->element[1][2]; d2 = m->element[1][3];
  128.  
  129.     a3 = m->element[2][0]; b3 = m->element[2][1]; 
  130.     c3 = m->element[2][2]; d3 = m->element[2][3];
  131.  
  132.     a4 = m->element[3][0]; b4 = m->element[3][1]; 
  133.     c4 = m->element[3][2]; d4 = m->element[3][3];
  134.  
  135.     ans = a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
  136.         - b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
  137.         + c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
  138.         - d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
  139.     return ans;
  140. }
  141.  
  142.  
  143.  
  144. /*
  145.  * double = det3x3(  a1, a2, a3, b1, b2, b3, c1, c2, c3 )
  146.  * 
  147.  * calculate the determinent of a 3x3 matrix
  148.  * in the form
  149.  *
  150.  *     | a1,  b1,  c1 |
  151.  *     | a2,  b2,  c2 |
  152.  *     | a3,  b3,  c3 |
  153.  */
  154.  
  155. double det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3 )
  156. double a1, a2, a3, b1, b2, b3, c1, c2, c3;
  157. {
  158.     double ans;
  159.     double det2x2();
  160.  
  161.     ans = a1 * det2x2( b2, b3, c2, c3 )
  162.         - b1 * det2x2( a2, a3, c2, c3 )
  163.         + c1 * det2x2( a2, a3, b2, b3 );
  164.     return ans;
  165. }
  166.  
  167. /*
  168.  * double = det2x2( double a, double b, double c, double d )
  169.  * 
  170.  * calculate the determinent of a 2x2 matrix.
  171.  */
  172.  
  173. double det2x2( a, b, c, d)
  174. double a, b, c, d; 
  175. {
  176.     double ans;
  177.     ans = a * d - b * c;
  178.     return ans;
  179. }
  180.  
  181.  
  182.